/*
 * Behavior.cpp
 *
 *  Created on: Dec 15, 2010
 *      Author: tgalluzzo
 */
#include "Behavior.h"
#include <boost/timer.hpp>

#define BEHAVIOR_ASSERT_NOT_INTERUPTED()                                \
{                                                                       \
    if(boost::this_thread::interruption_requested() || *interrupted)    \
    {                                                                   \
        *state = FAILED;                                                \
        return false;                                                   \
    }                                                                   \
}


Behavior::Behavior() :
  name("Behavior"),
  first(),
  second(),
  execute(),
  interrupted(new bool(false)),
  state(new State(NOT_RUN)),
  firstThread(),
  secondThread(),
  exitCondition(),
  mutex(),
  childrenRunningCount(new int(0)),
  executionTime_sec(new double(0))
{
}

Behavior::Behavior(const Behavior& other) :
  name(other.name),
  first(other.first),
  second(other.second),
  execute(other.execute),
  interrupted(other.interrupted),
  state(other.state),
  firstThread(other.firstThread),
  secondThread(other.secondThread),
  exitCondition(other.exitCondition),
  mutex(other.mutex),
  childrenRunningCount(other.childrenRunningCount),
  executionTime_sec(other.executionTime_sec)
{
    reset();
}

Behavior::Behavior(boost::function<bool (void)> newFunc) :
  name("Behavior"),
  first(),
  second(),
  execute(),
  interrupted(new bool(false)),
  state(new State(NOT_RUN)),
  firstThread(),
  secondThread(),
  exitCondition(),
  mutex(),
  childrenRunningCount(new int(0)),
  executionTime_sec(new double(0))
{
    execute = boost::bind(&Behavior::leafWrapper, *this, newFunc);
}

Behavior::Behavior(std::string newName, boost::function<bool (void)> newFunc) :
  name(newName),
  first(),
  second(),
  execute(),
  interrupted(new bool(false)),
  state(new State(NOT_RUN)),
  firstThread(),
  secondThread(),
  exitCondition(),
  mutex(),
  childrenRunningCount(new int(0)),
  executionTime_sec(new double(0))
{
    execute = boost::bind(&Behavior::leafWrapper, *this, newFunc);
}

Behavior::~Behavior()
{
    // Empty
}

void Behavior::reset()
{
    *state = NOT_RUN;
    *executionTime_sec = 0;
    *interrupted = false;
    if (first)
    {
        first->reset();
    }
    if (second)
    {
        second->reset();
    }
}

void Behavior::interrupt()
{
    std::cout << "Interrupt called on " << name << ", #" << this;
    *interrupted = true;
    if (first)
    {
        first->interrupt();
    }
    if (second)
    {
        second->interrupt();
    }
}

Behavior& Behavior::operator||(const Behavior &other)
{
    Behavior *newBehavior = new Behavior();
    newBehavior->name = "Select";
    newBehavior->execute = boost::bind(&Behavior::selector, newBehavior);
    newBehavior->first.reset(new Behavior(*this));
    newBehavior->second.reset(new Behavior(other));

    return *newBehavior;
}

Behavior& Behavior::operator&&(const Behavior &other)
{
    Behavior *newBehavior = new Behavior();
    newBehavior->name = "Sequence";
    newBehavior->execute = boost::bind(&Behavior::sequence, newBehavior);
    newBehavior->first.reset(new Behavior(*this));
    newBehavior->second.reset(new Behavior(other));

    return *newBehavior;
}

Behavior& Behavior::operator|(const Behavior &other)
{
    Behavior *newBehavior = new Behavior();
    newBehavior->name = "Or";
    newBehavior->execute = boost::bind(&Behavior::orboth, newBehavior);
    newBehavior->first.reset(new Behavior(*this));
    newBehavior->second.reset(new Behavior(other));

    return *newBehavior;
}

Behavior& Behavior::operator&(const Behavior &other)
{
    Behavior *newBehavior = new Behavior();
    newBehavior->name = "And";
    newBehavior->execute = boost::bind(&Behavior::andboth, newBehavior);
    newBehavior->first.reset(new Behavior(*this));
    newBehavior->second.reset(new Behavior(other));

    return *newBehavior;
}

//A+B&&C means always run a, and no matter what B returns, always run C
Behavior& Behavior::operator+(const Behavior &other)
{
    Behavior *newBehavior = new Behavior();
    newBehavior->name = "Plus";
    newBehavior->execute = boost::bind(&Behavior::plus, newBehavior);
    newBehavior->first.reset(new Behavior(*this));
    newBehavior->second.reset(new Behavior(other));

    return *newBehavior;
}

Behavior& Behavior::operator*(const Behavior &other)
{
    Behavior *newBehavior = new Behavior();
    newBehavior->name = "Parallel";
    newBehavior->execute = boost::bind(&Behavior::parallel, newBehavior);
    newBehavior->first.reset(new Behavior(*this));
    newBehavior->second.reset(new Behavior(other));

    return *newBehavior;
}

Behavior& Behavior::operator!(void)
{
    Behavior *newBehavior = new Behavior();
    newBehavior->name = "Not";
    newBehavior->execute = boost::bind(&Behavior::negate, newBehavior);
    newBehavior->first.reset(new Behavior(*this));
    return *newBehavior;
}

// Assignment operator for behavior display
Behavior& Behavior::operator=(const Behavior& other)
{
    name = other.name;
    first = other.first;
    second = other.second;
    execute = other.execute;
    interrupted = other.interrupted;
    state = other.state;
    firstThread = other.firstThread;
    secondThread = other.secondThread;
    exitCondition = other.exitCondition;
    runCondition = other.runCondition;
    mutex = other.mutex;
    childrenRunningCount = other.childrenRunningCount;
    executionTime_sec = other.executionTime_sec;
    return *this;
}

bool Behavior::announce(std::string announcement)
{
    boost::timer timer;
    *state = RUNNING;

    BEHAVIOR_ASSERT_NOT_INTERUPTED();

    bool succeeded = first->execute();

    *executionTime_sec = timer.elapsed();
    *state = succeeded ? PASSED : FAILED;
    return succeeded;
}

void Behavior::printTree(int level)
{
    std::cout << "PT: " << name << ", " << level << ", " << this;
    if (this->first.get())
    {
        first->printTree(level + 1);
    }
    if (this->second.get())
    {
        second->printTree(level + 1);
    }
}

bool Behavior::loopUntil(bool endCondition)
{
    boost::timer timer;
    *state = RUNNING;

    bool returnVal = !endCondition;
    while (returnVal != endCondition)
    {
        BEHAVIOR_ASSERT_NOT_INTERUPTED();
        std::cout << "Running loop: " << name;
        *first->state = RUNNING;
        returnVal = first->execute();
        *first->state = returnVal ? PASSED : FAILED;
    }

    *executionTime_sec = timer.elapsed();
    *state = returnVal == endCondition ? PASSED : FAILED;
    return returnVal;
}

bool Behavior::loop(int times)
{
    boost::timer timer;
    *state = RUNNING;

    bool returnVal = false;

    for (int i = 0; i < times; i++)
    {
        BEHAVIOR_ASSERT_NOT_INTERUPTED();std::cout << "Running loop: " << name;
        *first->state = RUNNING;
        returnVal = first->execute();
        *first->state = returnVal ? PASSED : FAILED;
        if (returnVal == true)
        {
            break;
        }
    }

    *executionTime_sec = timer.elapsed();
    *state = returnVal ? PASSED : FAILED;
    return returnVal;
}

bool Behavior::loopCondition(int times, EndCondition condition)
{
    boost::timer timer;
    *state = RUNNING;

    bool returnVal = false;

    for (int i = 0; i < times; i++)
    {
        BEHAVIOR_ASSERT_NOT_INTERUPTED();std::cout << "Running loopN: " << name;
        *first->state = RUNNING;
        returnVal = first->execute();
        *first->state = returnVal ? PASSED : FAILED;
        if (((returnVal == false) && (condition == WHEN_FALSE)) || ((returnVal == true) && (condition == WHEN_TRUE)))
        {
            break;
        }
    }

    *executionTime_sec = timer.elapsed();
    *state = returnVal ? PASSED : FAILED;
    return returnVal;
}

bool Behavior::negate()
{
    boost::timer timer;
    *state = RUNNING;

    BEHAVIOR_ASSERT_NOT_INTERUPTED();
    *first->state = RUNNING;
    bool returnVal = !first->execute();

    *executionTime_sec = timer.elapsed();
    *state = returnVal ? PASSED : FAILED;
    *first->state = !returnVal ? PASSED : FAILED;
    return returnVal;
}

bool Behavior::leafWrapper(Behavior& behavior, boost::function<bool(void)> leafFunc)
{
    boost::timer timer;
    *behavior.state = RUNNING;

    if(boost::this_thread::interruption_requested() || *behavior.interrupted)
    {
        *behavior.state = FAILED;
        return false;
    }

    bool returnVal = leafFunc();

    *behavior.executionTime_sec = timer.elapsed();
    *behavior.state = returnVal ? PASSED : FAILED;
    return returnVal;
}

bool Behavior::selector()
{
    boost::timer timer;
    *state = RUNNING;

    BEHAVIOR_ASSERT_NOT_INTERUPTED();
    *first->state = RUNNING;
    if (first->execute())
    {
        *executionTime_sec = timer.elapsed();
        *state = PASSED;
        *first->state = PASSED;
        return true;
    }
    *first->state = FAILED;

    BEHAVIOR_ASSERT_NOT_INTERUPTED();
    *second->state = RUNNING;
    bool returnVal = second->execute();

    *executionTime_sec = timer.elapsed();
    *state = returnVal ? PASSED : FAILED;
    *second->state = returnVal ? PASSED : FAILED;
    return returnVal;
}

bool Behavior::sequence()
{
    boost::timer timer;
    *state = RUNNING;

    BEHAVIOR_ASSERT_NOT_INTERUPTED();
    *first->state = RUNNING;
    if (!first->execute())
    {
        *executionTime_sec = timer.elapsed();
        *state = FAILED;
        *first->state = FAILED;
        return false;
    }
    *first->state = PASSED;

    BEHAVIOR_ASSERT_NOT_INTERUPTED();
    *second->state = RUNNING;
    bool returnVal = second->execute();

    *executionTime_sec = timer.elapsed();
    *state = returnVal ? PASSED : FAILED;
    *second->state = returnVal ? PASSED : FAILED;
    return returnVal;

}

bool Behavior::orboth()
{
    boost::timer timer;

    *state = RUNNING;

    BEHAVIOR_ASSERT_NOT_INTERUPTED();
    *first->state = RUNNING;
    bool firstResult = first->execute();
    *first->state = firstResult ? PASSED : FAILED;

    BEHAVIOR_ASSERT_NOT_INTERUPTED();
    *second->state = RUNNING;
    bool secondResult = second->execute();
    *second->state = secondResult ? PASSED : FAILED;

    bool returnVal = firstResult || secondResult;
    *state = returnVal ? PASSED : FAILED;
    *executionTime_sec = timer.elapsed();
    return returnVal;
}

bool Behavior::andboth()
{
    boost::timer timer;

    *state = RUNNING;

    BEHAVIOR_ASSERT_NOT_INTERUPTED();
    *first->state = RUNNING;
    bool firstResult = first->execute();
    *first->state = firstResult ? PASSED : FAILED;

    BEHAVIOR_ASSERT_NOT_INTERUPTED();
    *second->state = RUNNING;
    bool secondResult = second->execute();
    *second->state = secondResult ? PASSED : FAILED;

    bool returnVal = firstResult && secondResult;
    *state = returnVal ? PASSED : FAILED;
    *executionTime_sec = timer.elapsed();
    return returnVal;
}

bool Behavior::plus()
{
    boost::timer timer;
    *state = RUNNING;

    BEHAVIOR_ASSERT_NOT_INTERUPTED();
    *first->state = RUNNING;
    if (!first->execute())
    {
        *executionTime_sec = timer.elapsed();
        *state = FAILED;
        *first->state = FAILED;
        return false;
    }
    *first->state = PASSED;

    BEHAVIOR_ASSERT_NOT_INTERUPTED();
    *second->state = RUNNING;
    second->execute();

    *executionTime_sec = timer.elapsed();
    *state = PASSED;
    *second->state = PASSED;
    return true;
}

bool Behavior::parallel()
{
    boost::timer timer;
    *state = RUNNING;

    std::cout <<  "Executing behavior: " << name;

    *childrenRunningCount = 0;
    exitCondition.reset(new boost::condition());
    runCondition.reset(new boost::condition());
    mutex.reset(new boost::mutex());
    *first->state = RUNNING;
    firstThread.reset(new boost::thread(&Behavior::executeChildThread, this, first));
    *second->state = RUNNING;
    secondThread.reset(new boost::thread(&Behavior::executeChildThread, this, second));

    boost::posix_time::time_duration timeout = boost::posix_time::milliseconds(300);

    while (!boost::this_thread::interruption_requested() && (*first->state == RUNNING || *second->state == RUNNING))
    {
        boost::mutex::scoped_lock exitLock(*mutex);
        if (exitCondition->timed_wait(exitLock, timeout))
        {
            std::cout << "Exit condition signaled for behavior: " << name;
            break;
        }
    }

    bool successStatus = *childrenRunningCount ? false : true;

    firstThread->interrupt();
    firstThread->timed_join(timeout);

    secondThread->interrupt();
    secondThread->timed_join(timeout);

    *executionTime_sec = timer.elapsed();
    *state = successStatus ? PASSED : FAILED;
    return successStatus;
}

void Behavior::executeChildThread(BehaviorPtr child)
{
    mutex->lock();
    (*childrenRunningCount)++;
    mutex->unlock();

    std::cout <<  "Executing behavior in new thread: " << child->name;

    *child->state = RUNNING;
    if (!child->execute())
    {
        *child->state = FAILED;
        return;
    }
    *child->state = PASSED;

    (*childrenRunningCount)--;
    if (! *childrenRunningCount)
    {
        std::cout << "No more child nodes running";
        boost::mutex::scoped_lock exitLock(*mutex);
        exitCondition->notify_all();
    }
}
